#include "slave.h"

unsigned long rtc()
{
  unsigned long rpcc = 0;
  asm volatile ("rcsr %0, 4":"=&r"(rpcc)::"memory");
  return rpcc;
}


void slave_my_mutex_lock(volatile int* my_lock)
{
#ifdef TARG_SW5
#ifdef MUTEX_BASIC
    unsigned int __addr,__tmpi;
    __asm__ __volatile__(
        "        memb\n"
        "        ldi      %1,%2\n"
        "1:      faaw     %0,0(%1)\n"
        "        bne      %0,1b\n"
        "        memb\n"
      : "=&r"  (__tmpi),
        "=&r"  (__addr)
      :  "m" (*my_lock)
      : "memory");
    return;

#else
    unsigned int __tmp = 0;
    unsigned int __cnt;
    __asm__ __volatile__ (
    "0:     ldw     %[__tmp], %[my_lock]\n"
    "       beq     %[__tmp], 2f\n"
    "       ldi     %[__cnt], 50\n"
    "       sll     %[__cnt], 4, %[__cnt]\n"
    "1:     subw    %[__cnt], 1, %[__cnt]\n"
    "       bne     %[__cnt], 1b\n"
    "       br      0b\n"
    "2:     faaw    %[__tmp], %[my_lock]\n"
    "       bne     %[__tmp], 0b\n"
    "       memb    \n"
    "       br      3f\n"
    "3:     unop    \n"
    : [__tmp] "=&r"(__tmp), [__cnt] "=&r"(__cnt)
    : [my_lock] "m"(*(my_lock))
    : "memory");
    return;
#endif
#else 
#ifdef MUTEX_BASIC
    unsigned int __addr,__tmpi;
    __asm__ __volatile__(
        "        memb\n"
        "        ldi      %1,%2\n"
        "1:      faal     %0,0(%1)\n"
        "        bne      %0,1b\n"
        "        memb\n"
      : "=&r"  (__tmpi),
        "=&r"  (__addr)
      :  "m" (*my_lock)
      : "memory");
    return;

#else
    unsigned int __tmp = 0;
    unsigned int __cnt;
    __asm__ __volatile__ (
    "0:     ldl     %[__tmp], %[my_lock]\n"
    "       beq     %[__tmp], 2f\n"
    "       ldi     %[__cnt], 50\n"
    "       sll     %[__cnt], 4, %[__cnt]\n"
    "1:     subl    %[__cnt], 1, %[__cnt]\n"
    "       bne     %[__cnt], 1b\n"
    "       br      0b\n"
    "2:     faal    %[__tmp], %[my_lock]\n"
    "       bne     %[__tmp], 0b\n"
    "       memb    \n"
    "       br      3f\n"
    "3:     unop    \n"
    : [__tmp] "=&r"(__tmp), [__cnt] "=&r"(__cnt)
    : [my_lock] "m"(*(my_lock))
    : "memory");
    return;
#endif
#endif
}
void slave_my_mutex_unlock(volatile int* my_lock)
{
#ifdef TARG_SW5 
#ifdef MUTEX_BASIC
    unsigned int __addr ;
    __asm__ __volatile__(
        "        memb\n"
        "        ldi      %0,%1\n"
        "        stw      $31,0(%0)\n"
        "        memb\n"
      : "=&r"  (__addr)
      : "m" (*my_lock)
      : "memory");
    return;
#else
    unsigned int __tmp = 0;
    __asm__ __volatile__ (
    "       memb    \n"
    "       mov     0, %[__tmp]\n"
    "       stw     %[__tmp], %[my_lock]\n"
    : [__tmp] "=&r"(__tmp)
    : [my_lock] "m"(*(my_lock))
    : "memory"
    );
    return;
#endif
#else
  #ifdef MUTEX_BASIC
    unsigned int __addr ;
    __asm__ __volatile__(
        "        memb\n"
        "        ldi      %0,%1\n"
        "        stl      $31,0(%0)\n"
        "        memb\n"
      : "=&r"  (__addr)
      : "m" (*my_lock)
      : "memory");
    return;
#else
    unsigned int __tmp = 0;
    __asm__ __volatile__ (
    "       memb    \n"
    "       mov     0, %[__tmp]\n"
    "       stl     %[__tmp], %[my_lock]\n"
    : [__tmp] "=&r"(__tmp)
    : [my_lock] "m"(*(my_lock))
    : "memory"
    );
    return;
#endif
#endif
}
